#include <Tlc5940.h>
#include <tlc_config.h>
#include <Average.h>

int RED = 0;
int GREEN = 1;
int BLUE = 2;
int NONE = 8;
int MAXIMUM = 4095;

//From the bildr article: http://bildr.org/2012/03/servos-tlc5940-arduino
//Requires the Tlc5940 library. http://code.google.com/p/tlc5940arduino/downloads/list
byte address0 =    B00000000;
byte address1 =    B00000001;
byte address2 =    B00000010;
byte broadcastAddress = B00000011;
byte addressMask = B00000011;

byte on =     B00000100;
byte off =    B00000000;
byte onMask = B00000100;

int myAddress = address0; // hi

int unifyColor = NONE;

int s0 = 5;
int s1 = 6;
int s2 = 7;
int s3 = 8;
int count = 0;

int front = 0;
int back = 0;

int window = 0;
int value = 0;
int colorToChangeForWindow[5] = { 
  0, 0, 0, 0, 0 };
boolean dirs[5] = {
  false, false, false, false };
int colors[5] = {
  0, 1000, 2000, 3000, 4000 };

int averageSize = 100;
Average* frontAverage = newAverageWithSampleSize(averageSize);
Average* backAverage = newAverageWithSampleSize(averageSize);

Average* frontMaxAverage = newAverageWithSampleSize(1000);
Average* backMaxAverage = newAverageWithSampleSize(1000);

void setup() {  
  randomSeed(analogRead(4));
  initSensors();
  Tlc.init(4095);
  Serial.begin(9600);
  setAllWindowColor(1024, 0, 0);
  update();
  calibrateMaxDistance();
  setAllWindowColor(0, 1024, 0);
  update();
  delay(1000);
}

void calibrateMaxDistance() {
  for (int i = 0; i < 2000; i++) {
    int front = constrain(readMux(0), 22, 800);
    int back = constrain(readMux(1), 22, 800);
    frontMaxAverage->averageWithValue(front);
    backMaxAverage->averageWithValue(back);
    delay(1);
  }
}

void checkSerialInput() {
  if (Serial.available()) {
    byte data = Serial.read();
    byte dataAddress = data & addressMask;
    if (dataAddress == myAddress || dataAddress == broadcastAddress) {
      unifyColor = data & onMask;
    }
  }
}

void sendSerialData(int color) {
  byte data = broadcastAddress | color;
  Serial.write(data);
}

void initSensors() {  
  pinMode(s0, OUTPUT);
  pinMode(s1, OUTPUT);
  pinMode(s2, OUTPUT);
  pinMode(s3, OUTPUT); 

  digitalWrite(s0, LOW);
  digitalWrite(s1, LOW);
  digitalWrite(s2, LOW);
  digitalWrite(s3, LOW);
}

void readSensorData() {
    front = constrain(readMux(0), 22, 900);
    back = constrain(readMux(1), 22, 900);
    front = frontAverage->averageWithValue(front);
    back = backAverage->averageWithValue(back);
}

void loop() {
  //checkSerialInput();
  
  for (window = 0; window < 5; window++) {
    int red = 0;
    int green = 0;
    int blue = 0;

    int color = colors[window];
    if (dirs[window]) {
      colors[window] = color + 10;
    } 
    else {
      colors[window] = color - 10;
    }
    
    readSensorData();

    if (color > 4070) {
      dirs[window] = false;
    } 
    else if (color < 20) {
      dirs[window] = true;

      if (unifyColor == RED) {
        colorToChangeForWindow[window] = RED;
      } 
      else if (unifyColor == GREEN) {
        colorToChangeForWindow[window] = GREEN;
      } 
      else if (unifyColor == BLUE) {
        colorToChangeForWindow[window] = BLUE;
      } 
      else {
        // change which color to fade
        int rand = random(6);
        colorToChangeForWindow[window] = random(6);
      }

    }

    if (colorToChangeForWindow[window] == 0) {
      red = color;
    } 
    else if (colorToChangeForWindow[window] == 1) {
      green = color;
    } 
    else if (colorToChangeForWindow[window] == 2) {
      blue = color;
    } 
    else if (colorToChangeForWindow[window] == 3) {
      red = color;
      green = color;
    } 
    else if (colorToChangeForWindow[window] == 4) {
      red = color;
      blue = color;
    } 
    else {
      blue = color;
      green = color;
    }

    randomUnify();

    if (front < frontMaxAverage->average - 100) {
      blue = map(front, 20, frontMaxAverage->average, 4000, 0);
    }

    if (back < backMaxAverage->average - 100) {
      green = map(back, 20, backMaxAverage->average, 4000, 0);
    }

    setWindowColor(window, red, green, blue);
  }

  update();

  count++;
  if (count > 1000 + random(2000)) {
    count = 0;
    unifyColor = NONE;
  }
}

void randomUnify() {
    if (unifyColor == NONE && random(20000) == 0) {
      triggerUnifyColor();
      count = 0;
    }
}

void triggerUnifyColor() {
  int test = random(3);
  if (test == 0) {
    unifyColor = BLUE;
  } 
  else if (test == 1) {
    unifyColor = RED;
  } 
  else {
    unifyColor = GREEN;
  }
  sendSerialData(unifyColor);
}

void readSensor() {
  for(int i = 0; i < 10; i ++){
    Serial.print(i);
    Serial.print(": ");
    Serial.print(readMux(i));
    Serial.print("      ");
  } 
  Serial.println("");
  delay(300);
}

void setWindowColor(int window, int red, int green, int blue) {
  int redPin = getRedPin(window);
  int greenPin = getGreenPin(window);
  int bluePin = getBluePin(window);

  setColor(redPin, red);
  setColor(greenPin, green);
  setColor(bluePin, blue);
}

int getRedPin(int window) {
  int firstPin = window * 3;
  return firstPin;
}

int getGreenPin(int window) {
  int firstPin = window * 3;
  return firstPin + 1;
}

int getBluePin(int window) {
  int firstPin = window * 3;
  return firstPin + 2;
}

void update() {
  Tlc.update();
  delayMicroseconds(20);
}
void setAllWindowColor(int red, int green, int blue) {
  int window = 0;
  for (window = 0; window < 5; window++) {
    setWindowColor(window, red, green, blue);
  }
}

void setAllColor(int color) {
  int i;
  for (i = 0; i < 16; i++) {
    setColor(i, color);
  }
}

void setColor(int pin, int color) {
  Tlc.set(pin, MAXIMUM - color);
}

int readMux(int channel){
  int controlPin[] = {
    s0, s1, s2, s3                  };

  int muxChannel[16][4]={
    {
      0,0,0,0                                    }
    , //channel 0
    {
      1,0,0,0                                    }
    , //channel 1
    {
      0,1,0,0                                    }
    , //channel 2
    {
      1,1,0,0                                    }
    , //channel 3
    {
      0,0,1,0                                    }
    , //channel 4
    {
      1,0,1,0                                    }
    , //channel 5
    {
      0,1,1,0                                    }
    , //channel 6
    {
      1,1,1,0                                    }
    , //channel 7
    {
      0,0,0,1                                    }
    , //channel 8
    {
      1,0,0,1                                    }
    , //channel 9
    {
      0,1,0,1                                    }
    , //channel 10
    {
      1,1,0,1                                    }
    , //channel 11
    {
      0,0,1,1                                    }
    , //channel 12
    {
      1,0,1,1                                    }
    , //channel 13
    {
      0,1,1,1                                    }
    , //channel 14
    {
      1,1,1,1                                    }  //channel 15
  };

  //loop through the 4 sig
  for(int i = 0; i < 4; i ++){
    digitalWrite(controlPin[i], muxChannel[channel][i]);
  }

  //read the value at the SIG pin
  int val = analogRead(0);

  //return the value
  return val;
}

void initializeSonars() {
  // 0v logic zero initially
  pinMode(2, OUTPUT);  
  digitalWrite(2, LOW);
  delay(320);  
  pullSonarPin();
}

void pullSonarPin() {
  // pull pin "high"
  digitalWrite(2, HIGH);
  delayMicroseconds(25);
  pinMode(2, INPUT);  
  digitalWrite(2, HIGH);
}






